// Copyright 2016 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "basic_sprayer_interfaces/msg/steer.hpp"
#include "basic_sprayer_interfaces/msg/sprayer_status.hpp"
#include "basic_sprayer_interfaces/msg/four_wheel.hpp"

#include "modbus/modbus.h"

using namespace std::chrono_literals;

#define DIGITAL2REAL  (1/5000.0)
#define REAL2DIGITAL  5000.0   

/*
  订阅控制信息  和  发布车身的姿态信息
*/

/* This example creates a subclass of Node and uses std::bind() to register a
 * member function as a callback from the timer. */

class sprayer : public rclcpp::Node
{
public:
  sprayer()
  : Node("sprayer_node")
  {
    /* 连接串口 */
    ctx_sprayer = NULL;
    control_mode_set = mode_none;    //控制模式  默认为0
    //message.
    message.error = 0;
    ctx_sprayer = modbus_new_rtu("/dev/usb_1", 115200, 'N', 8, 1);
    if (ctx_sprayer == NULL) {
      message.error |= basic_sprayer_interfaces::msg::SprayerStatus::ERROR_NOT_CONNECT;          //创建与喷雾机通信设备错误
    }else{
        modbus_set_slave(ctx_sprayer, 0x2);
        if (modbus_connect(ctx_sprayer) == -1) {
            fprintf(stderr, "sprayer device connection failed: %s\n", modbus_strerror(errno));
            modbus_free(ctx_sprayer);
            ctx_sprayer = NULL;
            message.error |= basic_sprayer_interfaces::msg::SprayerStatus::ERROR_NOT_CONNECT;          //创建与喷雾机通信设备错误;      //喷雾机通信设备连接错误
        }
        modbus_set_response_timeout(ctx_sprayer,0,20000);  //200ms
        modbus_set_error_recovery(ctx_sprayer,MODBUS_ERROR_RECOVERY_PROTOCOL);
    }

    /*  发布车身姿态，v1 v2 v3 v4 angleF angleB */
    /*  订阅 cmd_steer 转向  cmd_pedal 油门  cmd_velocity 速度 */
    publisher_ = this->create_publisher<basic_sprayer_interfaces::msg::SprayerStatus>("sprayer_status", 10);    //

    timer_ = this->create_wall_timer(50ms, std::bind(&sprayer::timer_callback, this));
    
    subscription_steer = this->create_subscription<basic_sprayer_interfaces::msg::Steer>(
      "cmd_steer", 10, std::bind(&sprayer::steer_callback, this, std::placeholders::_1));

    subscription_pedal = this->create_subscription<basic_sprayer_interfaces::msg::FourWheel>(
      "cmd_pedal", 10, std::bind(&sprayer::pedal_callback, this, std::placeholders::_1));

    subscription_velocity = this->create_subscription<basic_sprayer_interfaces::msg::FourWheel>(
      "cmd_velocity", 10, std::bind(&sprayer::velocity_callback, this, std::placeholders::_1));
  }

private:  
  enum mode_enum{
    mode_none = (int32_t)0,
    mode_steer = 1,     // 实际要往寄存器内写入0
    mode_velocity = 2,  // 实际要往寄存器内写入1
    mode_pedal = 3,     // 实际要往寄存器内写入2
  };

  modbus_t *ctx_sprayer;   // 和大车进行通信

  mode_enum control_mode_set; 
  basic_sprayer_interfaces::msg::SprayerStatus message;    //车辆状态
  rclcpp::Subscription<basic_sprayer_interfaces::msg::Steer>::SharedPtr subscription_steer;
  rclcpp::Subscription<basic_sprayer_interfaces::msg::FourWheel>::SharedPtr subscription_pedal;
  rclcpp::Subscription<basic_sprayer_interfaces::msg::FourWheel>::SharedPtr subscription_velocity;

  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<basic_sprayer_interfaces::msg::SprayerStatus>::SharedPtr publisher_;

  void timer_callback()
  {
    uint16_t data[10];
    if( ctx_sprayer != NULL ){
        /* 读取信息 */
        message.error &= ~basic_sprayer_interfaces::msg::SprayerStatus::ERROR_NOT_CONNECT;
        if(modbus_read_registers(ctx_sprayer,19,6,data) != -1){
          message.lost_cnt = 0;
          message.angles[0] = -((int16_t)data[0])/1000.0;
          message.angles[1] = -((int16_t)data[1])/1000.0;

          message.wheel_velocity[0] = ((int16_t)data[2])*DIGITAL2REAL;
          message.wheel_velocity[1] = ((int16_t)data[3])*DIGITAL2REAL;
          message.wheel_velocity[2] = ((int16_t)data[4])*DIGITAL2REAL;
          message.wheel_velocity[3] = ((int16_t)data[5])*DIGITAL2REAL;
        }else{
          message.lost_cnt ++;
        }
    }else{
      message.error |= basic_sprayer_interfaces::msg::SprayerStatus::ERROR_NOT_CONNECT;
    }
    /* 读取信息 然后发布出去 */
    publisher_->publish(message);
  }

  void steer_callback(const basic_sprayer_interfaces::msg::Steer::SharedPtr msg)
  {
    switch( control_mode_set ){
      case mode_enum::mode_none:
        if(modbus_write_register(ctx_sprayer,0,0)!=-1){
            control_mode_set = mode_enum::mode_steer;
        }
      break;
      case mode_enum::mode_steer:
        {
          uint16_t array[2] = {0};
          array[0] = msg->velocity*REAL2DIGITAL;
          array[1] = - msg->angle*1000.0;
          modbus_write_registers(ctx_sprayer,25,2,(uint16_t *)array);          
        }
      break;
      default:    // 停下来
        
      break;
    }
  }

  void pedal_callback(const basic_sprayer_interfaces::msg::FourWheel::SharedPtr msg)
  {
    switch( control_mode_set ){
      case mode_enum::mode_none:
        if(modbus_write_register(ctx_sprayer,0,2)!=-1){
            control_mode_set = mode_enum::mode_pedal;
        }
      break;
      case mode_enum::mode_pedal:
        {
          uint16_t array[4] = {0};
          array[0] = msg->output_left_front*10;
          array[1] = msg->output_right_front*10;
          array[2] = msg->output_right_rear*10;
          array[3] = msg->output_left_rear*10;
          modbus_write_registers(ctx_sprayer,31,4,(uint16_t *)array);       
        }
      break;
      default:    // 停下来
        
      break;
    }
  }

  void velocity_callback(const basic_sprayer_interfaces::msg::FourWheel::SharedPtr msg) 
  {
    switch( control_mode_set ){
      case mode_enum::mode_none:
        if(modbus_write_register(ctx_sprayer,0,1)!=-1){
            control_mode_set = mode_enum::mode_velocity;
        }
      break;
      case mode_enum::mode_velocity:
        {
          uint16_t array[4] = {0};
          array[0] = msg->output_left_front*REAL2DIGITAL;
          array[1] = msg->output_right_front*REAL2DIGITAL;
          array[2] = msg->output_right_rear*REAL2DIGITAL;
          array[3] = msg->output_left_rear*REAL2DIGITAL;
          modbus_write_registers(ctx_sprayer,27,4,(uint16_t *)array);   
        }
      break;
      default:    // 停下来
        
      break;
    }
  }
};


int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<sprayer>());
  rclcpp::shutdown();
  return 0;
}
